#include <esp_now.h>
#include <WiFi.h>

// 电机引脚定义（四轮差速驱动）
#define MOTOR_A_IN1 25
#define MOTOR_A_IN2 16
#define MOTOR_B_IN1 18
#define MOTOR_B_IN2 17
#define MOTOR_C_IN1 21
#define MOTOR_C_IN2 19
#define MOTOR_D_IN1 23
#define MOTOR_D_IN2 22

// 摇杆数据结构体（发送端与接收端需一致）
typedef struct {
  int xValueJpyA;    // X轴值（0-4095）
  int yValueJpyA;    // Y轴值（0-4095）
  int xValueJpyB;    // X轴值（0-4095）
  int yValueJpyB;    // Y轴值（0-4095）
} JoystickData;
JoystickData joyData;

// 差速转向参数
const int compen = 12;    // 补偿值 （不动时推杆为 0，-12，0,0）
const int deadZone = 10;  //死区
const int baseSpeed = 150;   // 基础速度（0-255）

// 控制单个电机（速度范围：-255~+255）
void setMotor(int in1, int in2, int speed) {
    speed = constrain(speed, -255, 255);
    if (speed > 0) {
      analogWrite(in1, speed);
      analogWrite(in2, 0);
    } else {
      analogWrite(in1, 0);
      analogWrite(in2, -speed);
    }
  }

  // 差速控制函数（完整实现）
void differentialDrive(float throttle, float steering, int* leftSpeed, int* rightSpeed) {
  /* 参数说明：
     throttle : 油门量（-255~+255）
     steering : 转向量（-100~+100）
     leftSpeed: 输出左轮速度
     rightSpeed: 输出右轮速度
  */
  
  // 转向系数计算（限制在±1之间）
  float steeringFactor = constrain(steering, -100, 100) / 100.0f;

  // 差速计算（核心算法）
  *leftSpeed = throttle * (1.0f + steeringFactor);
  *rightSpeed = throttle * (1.0f - steeringFactor);

  // 动态限幅（保持比例关系）
  int maxVal = max(abs(*leftSpeed), abs(*rightSpeed));
  if (maxVal > 255) {
      float scale = 255.0f / maxVal;
      *leftSpeed *= scale;
      *rightSpeed *= scale;
  }
}

  /* 遥感
        100 
  100 ------ -100   x   
        -100
         y
  */
  
  // 接收回调函数
  void OnDataRecv(const uint8_t *mac, const uint8_t *data, int len) {
    memcpy(&joyData, data, len);

    // 在setup()中添加：
    Serial.print("结构体大小应为18，实际：");
    Serial.println(sizeof(joyData));
    
    // 获取遥感参数
    float xA = joyData.xValueJpyA+compen;  // A遥感补偿值
    float yA = joyData.yValueJpyA;  
    float xB = joyData.xValueJpyB;  
    float yB = joyData.yValueJpyB;  
    
    // 死区处理
    if (abs(xA) < deadZone) xA = 0;
    if (abs(yA) < deadZone) yA = 0;
    if (abs(xB) < deadZone) xB = 0;
    if (abs(yB) < deadZone) yB = 0;
    
    // 按照A遥感Y值作为速度值
    // 速度映射（保持yA的-100~100比例）
    float throttle = yA * (baseSpeed / 100.0);
    
    // 差速控制计算（新增）
    int leftFront, rightFront, leftRear, rightRear;
    differentialDrive(throttle, xB, &rightFront, &leftFront);
  
    // 四轮同步控制（前后轮同速）
    leftRear = leftFront;
    rightRear = rightFront;

    // 遥感B负责方向
    
    // 设置电机速度
    setMotor(MOTOR_A_IN1, MOTOR_A_IN2, leftFront);
    setMotor(MOTOR_B_IN1, MOTOR_B_IN2, rightFront);
    setMotor(MOTOR_C_IN1, MOTOR_C_IN2, leftRear);
    setMotor(MOTOR_D_IN1, MOTOR_D_IN2, rightRear);
  }
  

void setup() {
  Serial.begin(115200);
  // 初始化电机引脚为PWM输出
  pinMode(MOTOR_A_IN1, OUTPUT);
  pinMode(MOTOR_A_IN2, OUTPUT);
  
  pinMode(MOTOR_B_IN1, OUTPUT);
  pinMode(MOTOR_B_IN2, OUTPUT);

  pinMode(MOTOR_C_IN1, OUTPUT);
  pinMode(MOTOR_C_IN2, OUTPUT);

  pinMode(MOTOR_D_IN1, OUTPUT);
  pinMode(MOTOR_D_IN2, OUTPUT);
  
  // ESP-NOW初始化
  WiFi.mode(WIFI_STA);
  if (esp_now_init() != ESP_OK) {
    Serial.println("ESP-NOW初始化失败");
    return;
  }

  // 注册接收回调
  esp_now_register_recv_cb(OnDataRecv);
}


void loop() 
{
}
